#include "waypoint.h"
#include "aircraft.h" // include here and not in header due to cyclic dependencies
using namespace FsxSimConnect;

const double Waypoint::LatLngEpsilon = 1E-06;
const double Waypoint::AltFtEpsilon = 1E-01;
const double Waypoint::AngleRadEpsilon = 1E-03;
const double Waypoint::SpeedEpsilon = 2E-01;

//
// Default constructor for metadata
//
Waypoint::Waypoint(QObject *parent)  : QObject(parent),
    _latRad(0), _lngRad(0), _altFt(0), _headingTrueRad(0), _pitchRad(0), _bankRad(0),
    _groundspeedKts(0), _airspeedIndicatedKts(0), _airspeedTrueKts(0),
    _setInstrumentation(false), _setPositionAndSpeed(false), _timestampUtcEpoch(0),
    _onGnd(false), _isDelta(false)
{
    Waypoint::updateTimestamp();
}

//
// Constructor
//
Waypoint::Waypoint(double lat, double lng, double altFt, double heading, double pitch, double bank,
                   double gs, double ias, double tas, bool onGnd, QObject *parent) : QObject(parent),
    _latRad(lat), _lngRad(lng), _altFt(altFt), _headingTrueRad(heading), _pitchRad(pitch), _bankRad(bank),
    _groundspeedKts(gs), _airspeedIndicatedKts(ias), _airspeedTrueKts(tas),
    _setInstrumentation(false), _setPositionAndSpeed(false), _timestampUtcEpoch(0),
    _onGnd(onGnd), _isDelta(false)
{
    Waypoint::updateTimestamp();
}

//
// Constructor
//
Waypoint::Waypoint(Aircraft* aircraft, QObject *parent) : QObject(parent),
    _latRad(0), _lngRad(0), _altFt(-1), _headingTrueRad(0), _pitchRad(0), _bankRad(0),
    _groundspeedKts(-1), _airspeedIndicatedKts(-1), _airspeedTrueKts(-1),
    _setInstrumentation(false), _setPositionAndSpeed(false), _timestampUtcEpoch(0),
    _onGnd(false), _isDelta(false)
{
    _timestampUtcEpoch = aircraft->getLastUpdatedTimestampUtcEpoch();
    FSXAircraftPositionAndSpeedData ps = aircraft->getPositionSpeedData();
    Waypoint::update(ps);
    FSXAircraftInstrumentationData id = aircraft->getInstrumentationData();
    Waypoint::update(id);
}

//
// Constructor
//
Waypoint::Waypoint(const FSXAircraftPositionAndSpeedData& pas, qint64 timestamp, QObject *parent) : QObject(parent),
    _latRad(0), _lngRad(0), _altFt(-1), _headingTrueRad(0), _pitchRad(0), _bankRad(0),
    _groundspeedKts(-1), _airspeedIndicatedKts(-1), _airspeedTrueKts(-1),
    _setInstrumentation(false), _setPositionAndSpeed(false), _timestampUtcEpoch(0),
    _onGnd(false), _isDelta(false)
{
    Waypoint::update(pas);
    if (timestamp <= 0)
        Waypoint::updateTimestamp();
    else
        _timestampUtcEpoch = timestamp;
}

//
// Constructor
//
Waypoint::Waypoint(const FSXAircraftInstrumentationData& id, qint64 timestamp, QObject *parent) : QObject(parent),
    _latRad(0), _lngRad(0), _altFt(-1), _headingTrueRad(0), _pitchRad(0), _bankRad(0),
    _groundspeedKts(-1), _airspeedIndicatedKts(-1), _airspeedTrueKts(-1),
    _setInstrumentation(false), _setPositionAndSpeed(false),
    _onGnd(false), _isDelta(false), _timestampUtcEpoch(0)
{
    Waypoint::update(id);
    if (timestamp <= 0)
        Waypoint::updateTimestamp();
    else
        _timestampUtcEpoch = timestamp;
}

//
// Delta constructor
//
Waypoint::Waypoint(const Waypoint &newWaypoint, const Waypoint &oldWaypoint, QObject *parent) : QObject(parent),
    _latRad(newWaypoint._latRad-oldWaypoint._latRad), _lngRad(newWaypoint._lngRad-oldWaypoint._lngRad), _altFt(newWaypoint._altFt-oldWaypoint._altFt),
    _pitchRad(newWaypoint._pitchRad-oldWaypoint._pitchRad), _bankRad(newWaypoint._bankRad-oldWaypoint._bankRad), _headingTrueRad(newWaypoint._headingTrueRad-oldWaypoint._headingTrueRad),
    _airspeedIndicatedKts(newWaypoint._airspeedIndicatedKts-oldWaypoint._airspeedIndicatedKts), _airspeedTrueKts(newWaypoint._airspeedTrueKts-oldWaypoint._airspeedTrueKts),
    _groundspeedKts(newWaypoint._groundspeedKts-oldWaypoint._groundspeedKts), _timestampUtcEpoch(newWaypoint._timestampUtcEpoch-oldWaypoint._timestampUtcEpoch),
    _setInstrumentation(true), _setPositionAndSpeed(true),
    _onGnd(newWaypoint._onGnd == oldWaypoint._onGnd), _isDelta(true)
{
    // code goes here
}

//
// Copy constructor
//
Waypoint::Waypoint(const Waypoint &otherWaypoint) :
    _latRad(otherWaypoint._latRad), _lngRad(otherWaypoint._lngRad), _altFt(otherWaypoint._altFt),
    _pitchRad(otherWaypoint._pitchRad), _bankRad(otherWaypoint._bankRad), _headingTrueRad(otherWaypoint._headingTrueRad),
    _airspeedIndicatedKts(otherWaypoint._airspeedIndicatedKts), _airspeedTrueKts(otherWaypoint._airspeedTrueKts),
    _groundspeedKts(otherWaypoint._groundspeedKts), _timestampUtcEpoch(otherWaypoint._timestampUtcEpoch),
    _setInstrumentation(otherWaypoint._setInstrumentation), _setPositionAndSpeed(otherWaypoint._setPositionAndSpeed),
    _onGnd(otherWaypoint._onGnd), _isDelta(otherWaypoint._isDelta)
{
    // code goes here
}

//
// Assignment operator
//
Waypoint& Waypoint::operator=(const Waypoint &otherWaypoint) {

    // Check for self-assignment!
    if (this == &otherWaypoint)  return *this; // Same object? Yes, so skip assignment, and just return *this
    _latRad = otherWaypoint._latRad;
    _lngRad = otherWaypoint._lngRad;
    _altFt = otherWaypoint._altFt;
    _headingTrueRad = otherWaypoint._headingTrueRad;
    _pitchRad = otherWaypoint._pitchRad;
    _bankRad = otherWaypoint._bankRad;
    _airspeedIndicatedKts = otherWaypoint._airspeedIndicatedKts;
    _airspeedTrueKts = otherWaypoint._airspeedTrueKts;
    _groundspeedKts = otherWaypoint._groundspeedKts;
    _timestampUtcEpoch = otherWaypoint._timestampUtcEpoch;
    _setInstrumentation = otherWaypoint._setInstrumentation;
    _setPositionAndSpeed = otherWaypoint._setPositionAndSpeed;
    _isDelta = otherWaypoint._isDelta;
    _onGnd = otherWaypoint._onGnd;
    return *this;
}

//
// Add values of 2 waypoints
//
Waypoint Waypoint::operator +(const Waypoint &otherWaypoint)
{
    // http://courses.cms.caltech.edu/cs11/material/cpp/donnie/cpp-ops.html
    Waypoint result = *this; // Make a copy of myself.  Same as MyClass result(*this);
    result += otherWaypoint; // Use += to add other to the copy.
    return result;
}

//
// Substract values of 2 waypoints
//
Waypoint Waypoint::operator -(const Waypoint &otherWaypoint)
{
    // http://courses.cms.caltech.edu/cs11/material/cpp/donnie/cpp-ops.html
    Waypoint result = *this; // Make a copy of myself.  Same as MyClass result(*this);
    result -= otherWaypoint; // Use -= to add other to the copy.
    return result;
}

//
// Add values of another waypoint
//
Waypoint &Waypoint::operator +=(const Waypoint &otherWaypoint)
{
    _latRad += otherWaypoint._latRad;
    _lngRad += otherWaypoint._lngRad;
    _altFt  += otherWaypoint._altFt;
    _headingTrueRad += otherWaypoint._headingTrueRad;
    _pitchRad += otherWaypoint._pitchRad;
    _bankRad += otherWaypoint._bankRad;
    _airspeedIndicatedKts += otherWaypoint._airspeedIndicatedKts;
    _airspeedTrueKts += otherWaypoint._airspeedTrueKts;
    _groundspeedKts += otherWaypoint._groundspeedKts;
    _timestampUtcEpoch += otherWaypoint._timestampUtcEpoch;
    _setInstrumentation = otherWaypoint._setInstrumentation || _setInstrumentation;
    _setPositionAndSpeed = otherWaypoint._setPositionAndSpeed || _setPositionAndSpeed;
    _isDelta = false;
    _onGnd = _onGnd || otherWaypoint._onGnd;
    return *this;
}

//
// Substract values of another waypoint
//
Waypoint &Waypoint::operator -=(const Waypoint &otherWaypoint)
{
    _latRad -= otherWaypoint._latRad;
    _lngRad -= otherWaypoint._lngRad;
    _altFt  -= otherWaypoint._altFt;
    _headingTrueRad -= otherWaypoint._headingTrueRad;
    _pitchRad -= otherWaypoint._pitchRad;
    _bankRad -= otherWaypoint._bankRad;
    _airspeedIndicatedKts -= otherWaypoint._airspeedIndicatedKts;
    _airspeedTrueKts -= otherWaypoint._airspeedTrueKts;
    _groundspeedKts -= otherWaypoint._groundspeedKts;
    _timestampUtcEpoch -= otherWaypoint._timestampUtcEpoch;
    _setInstrumentation = otherWaypoint._setInstrumentation || _setInstrumentation;
    _setPositionAndSpeed = otherWaypoint._setPositionAndSpeed || _setPositionAndSpeed;
    _isDelta = false;
    _onGnd = _onGnd && otherWaypoint._onGnd;
    return *this;
}

//
// Divide values of another
//
Waypoint &Waypoint::operator /=(const int divisor)
{
    _latRad /= divisor;
    _lngRad /= divisor;
    _altFt  /= divisor;
    _headingTrueRad /= divisor;
    _pitchRad /= divisor ;
    _bankRad  /= divisor;
    _airspeedIndicatedKts /= divisor;
    _airspeedTrueKts /= divisor;
    _groundspeedKts  /= divisor;
    return *this;
}

//
// Divide values
//
Waypoint Waypoint::operator /(const int divisor)
{
    // http://courses.cms.caltech.edu/cs11/material/cpp/donnie/cpp-ops.html
    Waypoint result = *this;
    result /= divisor;
    return result;
}

//
// When last updated?
//
void Waypoint::updateTimestamp() {
    Waypoint::_timestampUtcEpoch = QDateTime::currentMSecsSinceEpoch();
}

//
// Within a given timeframe?
//
bool Waypoint::isWithinTimeFrame(const qint64 frameMs) const
{
    if (frameMs < 0) return false;
    qint64 deltaTime = QDateTime::currentMSecsSinceEpoch() - Waypoint::_timestampUtcEpoch;
    return (deltaTime <= WPTIMEFRAMEMIN || deltaTime <= frameMs);
}

//
// Has position been changed?
//
bool Waypoint::hasPositionChanged(const Waypoint &otherWp)
{
    if (abs(otherWp._latRad - _latRad) > LatLngEpsilon) return true;
    if (abs(otherWp._lngRad - _lngRad) > LatLngEpsilon) return true;
    return (abs(otherWp._altFt - _altFt) > AltFtEpsilon);
}

//
// Flight state changed
//
bool Waypoint::hasFlightStateChanged(const Waypoint &otherWp)
{
    if (abs(otherWp._bankRad - _bankRad) > AngleRadEpsilon) return true;
    if (abs(otherWp._headingTrueRad - _headingTrueRad) > AngleRadEpsilon) return true;
    return (abs(otherWp._pitchRad - _pitchRad) > AngleRadEpsilon);
}

//
// Flight state changed
//
bool Waypoint::hasSpeedChanged(const Waypoint &otherWp)
{
    if (!Waypoint::onGnd()) {
        // consider this only airborn, otherwise we get very frequent updates
        // due to
        if (abs(otherWp._airspeedIndicatedKts - _airspeedIndicatedKts) > SpeedEpsilon) return true;
        if (abs(otherWp._airspeedTrueKts - _airspeedTrueKts) > SpeedEpsilon) return true;
    }
    return (abs(otherWp._groundspeedKts - _groundspeedKts) > Waypoint::SpeedEpsilon);
}

//
// Changed state?
//
bool Waypoint::hasChanged(const Waypoint &otherWp) {
    // valid speed / position?
    if (otherWp._setPositionAndSpeed && Waypoint::_setPositionAndSpeed) {
        if (Waypoint::hasPositionChanged(otherWp)) return true;
        if (Waypoint::hasSpeedChanged(otherWp)) return true;
    }
    // valid instrumentation?
    if (otherWp._setInstrumentation && Waypoint::_setInstrumentation) {
        if (Waypoint::hasFlightStateChanged(otherWp)) return true;
    }
    return false;
}

//
// Update
//
void Waypoint::update(const FSXAircraftPositionAndSpeedData &sd)
{
    _latRad = sd.planeLatitudeRad;
    _lngRad = sd.planeLongitudeRad;
    _altFt = sd.planeAltitudeFt;
    _bankRad = sd.planeBankRad;
    _headingTrueRad = sd.planeHeadingTrueRad;
    _groundspeedKts = sd.groundVelocityKts;
    _pitchRad = sd.planePitchRad;
    _onGnd = sd.simOnGround;
    _setPositionAndSpeed = true;
    if (_timestampUtcEpoch <= 0) Waypoint::updateTimestamp();
}

//
// Update
//
void Waypoint::update(const FSXAircraftInstrumentationData &id)
{
    _airspeedIndicatedKts = id.airspeedIndicatedKts;
    _airspeedTrueKts = id.airspeedTrueKts;
    _setInstrumentation = true;
    if (_timestampUtcEpoch <= 0) Waypoint::updateTimestamp();
}
